/*
 * velocity.c
 *
 *  Created on: May 23, 2021
 *      Author: kirk_
 */

#include "velocity.h"

void VelocityInit(void* argument)
{
	uint32_t Period;
	int i;
	/* Get arg */
	if(argument != NULL)
		Period = *(uint32_t*)argument;
	else
		Period = Default_Fetch_Period;
	/* Init params */
	for(i=0;i<4;i++)
	{
		KD_FloatPID_Init(&Velocity[i], VelocityDownlink);
		KD_FloatPID_SetIndex(&Velocity[i], KD_FloatPID_IndexP, 0.00);
		KD_FloatPID_SetIndex(&Velocity[i], KD_FloatPID_IndexI, 0.00);
		KD_FloatPID_SetIndex(&Velocity[i], KD_FloatPID_IndexD, 0.00);
		KD_FloatPID_SetIndex(&Velocity[i], KD_FloatPID_Target, 0.00);
		KD_FloatPID_SetIndex(&Velocity[i], KD_FloatPID_Tolerance, 0.00);
	}
	/* Test param */
	for(i=0;i<4;i++)
	{
		Velocity[i].Denominator = -Period;
		Velocity[i].indexP = 0.70;
		Velocity[i].indexI = 0.32;
		Velocity[i].indexD = 0.32;
		Velocity[i].Tolerance = 0.01;
		Velocity[i].Target = 0.00;
	}

	/* Init motors */
	KDL_TB6612_Init(&Motor[0]);
	KDL_TB6612_SetPort(&Motor[0], &htim15, TIM_CHANNEL_1, KDL_TB6612_PortComplement);
	KDL_TB6612_SetPin1(&Motor[0], A_IN1_GPIO_Port, A_IN1_Pin);
	KDL_TB6612_SetPin2(&Motor[0], A_IN2_GPIO_Port, A_IN2_Pin);
	// KDL_TB6612_SetSpeed(&Motor[0], -0.2);

	KDL_TB6612_Init(&Motor[1]);
	KDL_TB6612_SetPort(&Motor[1], &htim15, TIM_CHANNEL_2, KDL_TB6612_PortGeneral);
	KDL_TB6612_SetPin1(&Motor[1], B_IN1_GPIO_Port, B_IN1_Pin);
	KDL_TB6612_SetPin2(&Motor[1], B_IN2_GPIO_Port, B_IN2_Pin);
	// KDL_TB6612_SetSpeed(&Motor[1], -0.2);

	KDL_TB6612_Init(&Motor[2]);
	KDL_TB6612_SetPort(&Motor[2], &htim5, TIM_CHANNEL_3, KDL_TB6612_PortGeneral);
	KDL_TB6612_SetPin1(&Motor[2], C_IN1_GPIO_Port, C_IN1_Pin);
	KDL_TB6612_SetPin2(&Motor[2], C_IN2_GPIO_Port, C_IN2_Pin);
	// KDL_TB6612_SetSpeed(&Motor[2], -0.2);

	KDL_TB6612_Init(&Motor[3]);
	KDL_TB6612_SetPort(&Motor[3], &htim5, TIM_CHANNEL_4, KDL_TB6612_PortGeneral);
	KDL_TB6612_SetPin1(&Motor[3], D_IN1_GPIO_Port, D_IN1_Pin);
	KDL_TB6612_SetPin2(&Motor[3], D_IN2_GPIO_Port, D_IN2_Pin);
	// KDL_TB6612_SetSpeed(&Motor[3], -0.2);

}


void VelocitySetTarget(int index, float target)
{
	/* Limit range */
	//if(target > 1.00) target = 1.00;
	//if(target < -1.00) target = -1.00;
	/* Set target */
	Velocity[index].Target = target;
}

void VelocityIncTarget(int index, float inc)
{
	VelocitySetTarget(index, Velocity[index].Target + inc);
}

float VelocityConvert(int32_t velocity)
{
	return ((float)velocity) / ((float)Max_Velocity);
}

void VelocityDownlink(KD_FloatPID_t* pid)
{
	int i;
	float ftmp, limit;
	for(i=0;i<4;i++)
	{
		if(pid == &Velocity[i])
		{
			limit = pid->Period;
			limit /= 1000.00;
			limit *= Velocity_Inc_Limit;
			ftmp = pid->Increment;
			if(ftmp > limit) ftmp = limit;
			if(ftmp < -limit) ftmp = -limit;
			KDL_TB6612_IncSpeed(&Motor[i], ftmp);
		}
	}
	return;
}
